#include "BaseMissile.h"
#include "BtOgrePG.h"
#include "BtOgreGP.h"
#include "BtOgreExtras.h"
#include "OgreConsole.h"
#include "BaseCannon.h"
#include <OgreSubEntity.h>
#include "PhysicsManager.h"
#include "GameState.hpp"

BaseMissile::BaseMissile(Ogre::SceneManager* sceneMgr)
{
	m_MoveSpeed = 0.5;
	m_MaxSpeed = 5;
	m_MaxRotateSpeed = 0.4;
	m_RotateSpeed = 0.1;

	//OgreConsole::getSingleton().print("Missile created");
	m_pSceneMgr = sceneMgr;

	m_MissileEntity =  m_pSceneMgr->createEntity("lowpolybullet.mesh");
	m_MissileEntity ->setMaterialName("Bullet_UV");
	m_MissileNode = m_pSceneMgr->getRootSceneNode()->createChildSceneNode();

	m_MissileNode->attachObject(m_MissileEntity);
	m_MissileNode->setPosition(Vector3(50, 0, -50));
	
	//Create shape.
	BtOgre::StaticMeshToShapeConverter converter(m_MissileEntity);
    m_MissileShape = converter.createBox();

	m_TranslateVector = Vector3::ZERO;
	m_RotateVector = Vector3::ZERO;
	m_MissileNode->setScale(0.1, 0.1, 0.3);

    //Calculate inertia.
    btScalar mass = 5;
    btVector3 inertia = btVector3(1,1,1);
    m_MissileShape->calculateLocalInertia(mass, inertia);

    //Create BtOgre MotionState (connects Ogre and Bullet).
    m_MissileState = new BtOgre::RigidBodyState(m_MissileNode);

    //Create the Body.
    m_MissileBody = new btRigidBody(mass, m_MissileState, m_MissileShape, inertia);

	PhysicsManager::getSingletonPtr()->addPhysicsProjectile(m_MissileBody);

	m_MissileNode->setVisible(false);
	isActive = false;
	aliveTimer = new Ogre::Timer();
	aliveTimerMax = 6000;
}


BaseMissile::~BaseMissile(void)
{
}

void BaseMissile::updateMissile(double timeSinceLastFrame)
{
	if(!isActive)
		return;

	m_MoveScale = m_MoveSpeed  * timeSinceLastFrame;
    m_RotScale  = m_RotateSpeed * timeSinceLastFrame;
	
	m_TranslateVector = Vector3(0, 0, -m_MoveScale);

	m_MissileBody->clearForces();
	//btQuaternion temp = m_MissileBody->getOrientation();
	
	Ogre::Vector3 UserForce = m_TranslateVector;
	btVector3 relativeForce = (btVector3(UserForce.x, 0 ,UserForce.z));

	//Gets translation vector relative to direction
	btMatrix3x3& boxRot = m_MissileBody->getWorldTransform().getBasis();
	btVector3 correctedForce = boxRot * relativeForce;

	m_MissileBody->setActivationState(DISABLE_DEACTIVATION);
	m_MissileBody->applyForce(correctedForce,btVector3(0,0,0));

	Ogre::Vector3 RotateForce = m_RotateVector;
	m_MissileBody->applyForce(btVector3(RotateForce.x, 0 , 0),btVector3(0,0,1));
	m_MissileBody->applyForce(btVector3(-RotateForce.x, 0 , 0),btVector3(0,0,-1));

	m_MissileState->setWorldTransform(m_MissileBody->getWorldTransform());

	//Resets vectors
	m_TranslateVector = Vector3::ZERO;
	m_RotateVector = Vector3::ZERO;

	
	//Does the missle need to turn?
	Vector3 mDestination = m_MissileTarget;                    // mDestination is the next location
	Vector3 mDirection = mDestination - m_MissileNode->getPosition();     // B-A = A->B (see vector questions above)
	Vector3 src = m_MissileNode->_getDerivedOrientation() * -Vector3::UNIT_Z;      // Orientation from initial direction
	src.y = 0;                                                    // Ignore pitch difference angle
	mDirection.y = 0;
	src.normalise();
	Real mDistance = mDirection.normalise( );                     // Both vectors modified so renormalize them
	Quaternion quat = src.getRotationTo(mDirection);
	
	Quaternion currentOrientation = m_MissileNode->getOrientation();

	Quaternion targetingRot;

	targetingRot = currentOrientation * quat;
	Radian yRad;
	Radian pRad;
	Radian rRad;

	Ogre::Matrix3 mat;
	targetingRot.ToRotationMatrix(mat);
	mat.ToEulerAnglesYXZ(yRad, pRad, rRad);
	Degree yDeg = yRad;
	Degree xDeg = pRad;
	Degree zDeg = rRad;

	//find a target
	Quaternion orientSrc = m_MissileNode->getOrientation();
    Quaternion orientDest = targetingRot;

    Quaternion delta = Quaternion::nlerp(m_RotateSpeed, orientSrc, orientDest, true);

	m_MissileBody->setWorldTransform(btTransform(btQuaternion(delta.x,delta.y,delta.z,delta.w), m_MissileBody->getWorldTransform().getOrigin()));

	tickCallBack();
}

void BaseMissile::tickCallBack()
{
	 // mShipBody is the spaceship's btRigidBody
    btVector3 velocity = m_MissileBody->getLinearVelocity();
    btScalar speed = velocity.length();
    if(speed > m_MaxSpeed) {
        velocity *= m_MaxSpeed/speed;
        m_MissileBody->setLinearVelocity(velocity);
    }

	btVector3 angleVelocity = m_MissileBody->getAngularVelocity();
    btScalar angleSpeed = angleVelocity.length();
    if(angleSpeed > m_MaxRotateSpeed) {
        angleVelocity *= m_MaxRotateSpeed/angleSpeed;
        m_MissileBody->setAngularVelocity(angleVelocity);
    }
}

void BaseMissile::updateTarget(Ogre::Vector3 target)
{
	m_MissileTarget = target;
}

void BaseMissile::rotateLeft()
{
	m_RotateVector.x = m_RotScale;
}
void BaseMissile::rotateRight()
{
	 m_RotateVector.x = -m_RotScale;
}

bool BaseMissile::getActiveState()
{
	return isActive;
}

void BaseMissile::setActiveState(bool state)
{
	isActive = state;

	if(!isActive){
		m_MissileNode->setVisible(false);
	}
}

void BaseMissile::resetMissile(Vector3 pPos, Quaternion pOrientation, Vector3 target){

	this->setActiveState(true);
	//m_MissileNode->setPosition(pPos);
	//m_MissileNode->setOrientation(pOrientation);
	//m_MissileNode->yaw((Degree)180);
	m_MissileBody->clearForces();
	m_MissileTarget = target;
	m_MissileBody->setWorldTransform(btTransform(btQuaternion(pOrientation.x,pOrientation.y,pOrientation.z,pOrientation.w), btVector3(pPos.x,pPos.y,pPos.z)));
	m_MissileNode->setVisible(true);
	aliveTimer = new Ogre::Timer();

}